#include <Arduino.h>
#include <WiFi.h>
#include <stdio.h>

#include "esp_now_helper.h"
#include "simple_string.h"
#define TCP_SERVER_ADDR "bemfa.com"

#define TCP_SERVER_PORT "8344"
#define DEFAULT_STASSID "xxxx"
#define DEFAULT_STAPSW "xxxxx"
#define UID "xxxxx"
#define TOPIC "car"

const char *ntpServer = "pool.ntp.org";
const long gmtOffset_sec = 8 * 3600;
const int daylightOffset_sec = 0;

#define MAX_PACKETSIZE 512
#define KEEPALIVEATIME 30 * 1000
#define LED_IVEATIME 1 * 1000
#define SEND_IVEATIME 3000

#define ANALOG_MAX 2830
#define ANALOG_HIGH 2000
#define ANALOG_LOW 1000
#define ANALOG_MIN 20

WiFiClient TCPclient;
char TcpClient_Buff[MAX_PACKETSIZE + 1] = {0};
unsigned int TcpClient_BuffIndex = 0;
unsigned long preHeartTick = 0;     // 心跳
unsigned long sendTick = 0;         // 发送时间
unsigned long preTCPStartTick = 0;  // 连接
bool preTCPConnected = false;

// 前后
int fw_value = 0;
// 左右
int lr_value = 0;

int beep = false;

// 连接WIFI
#ifndef __USE_ESP_NOW__
void doWiFiTick();
void startSTA();

// TCP初始化连接
void doTCPClientTick();
void startTCPClient();
void sendtoTCPServer(String p);
#endif
#ifndef __USE_ESP_NOW__
/*
 *发送数据到TCP服务器
 */
void sendtoTCPServer(String p) {
  if (!TCPclient.connected()) {
    Serial.println("Client is not readly");
    return;
  }
  TCPclient.print(p);
  Serial.println("[Send to TCPServer]:String");
  Serial.println(p);
}

/*
 *初始化和服务器建立连接
 */
void startTCPClient() {
  if (TCPclient.connect(TCP_SERVER_ADDR, atoi(TCP_SERVER_PORT))) {
    Serial.print("\nConnected to server:");
    Serial.printf("%s:%d\r\n", TCP_SERVER_ADDR, atoi(TCP_SERVER_PORT));
    char tcpTemp[128];
    sprintf(tcpTemp, "cmd=1&uid=%s&topic=%s\r\n", UID, TOPIC);

    sendtoTCPServer(tcpTemp);
    preTCPConnected = true;
    preHeartTick = millis();
    TCPclient.setNoDelay(true);
  } else {
    Serial.print("Failed connected to server:");
    Serial.println(TCP_SERVER_ADDR);
    TCPclient.stop();
    preTCPConnected = false;
  }
  preTCPStartTick = millis();
}

/*
 *检查数据，发送心跳
 */
void doTCPClientTick() {
  // 检查是否断开，断开后重连
  if (WiFi.status() != WL_CONNECTED) return;

  if (!TCPclient.connected()) {  // 断开重连

    if (preTCPConnected == true) {
      preTCPConnected = false;
      preTCPStartTick = millis();
      Serial.println();
      Serial.println("TCP Client disconnected.");
      TCPclient.stop();
    } else if (millis() - preTCPStartTick > 1 * 1000)  // 重新连接
      startTCPClient();
  } else {
    // timeClient.update();
    while (TCPclient.available()) {  // 收数据
      char c = TCPclient.read();
      TcpClient_Buff[TcpClient_BuffIndex] = c;
      TcpClient_BuffIndex++;

      preHeartTick = millis();
      if ((TcpClient_BuffIndex > 2 &&
           (TcpClient_Buff[TcpClient_BuffIndex - 1] == '\n' &&
            TcpClient_Buff[TcpClient_BuffIndex - 2] == '\r'))) {  // data ready
        TCPclient.flush();
        // printf("Buff:%s\n", TcpClient_Buff);
        // if ((TcpClient_Buff.indexOf("&msg=on") > 0)) {
        //   // turnOnLed();
        // } else if ((TcpClient_Buff.indexOf("&msg=off") > 0)) {
        //   // turnOffLed();
        // }
        str_section sections[4];
        str_section sections2[2];
        str_section sections3[3];
        int count = str_split(TcpClient_Buff, TcpClient_BuffIndex + 1, '&',
                              sections, 4);
        char out_buffer[100];
        char key_buffer[10];
        char value_buffer[100];
        char item_buffer[5];

        for (int i = 0; i < count; i++) {
          // printf("%d:%d\n", sections[i].begin, sections[i].end);
          section_str(sections[i], TcpClient_Buff, out_buffer,
                      sizeof(out_buffer));
          // printf("%s\n", out_buffer);
          int count2 =
              str_split(out_buffer, sections[i].end - sections[i].begin, '=',
                        sections2, 2);
          if (count2 == 2) {
            section_str(sections2[0], out_buffer, key_buffer,
                        sizeof(key_buffer));
            if (strcmp(key_buffer, "msg") == 0) {
              section_str(sections2[1], out_buffer, value_buffer,
                          sizeof(value_buffer));
              // printf("%s\n", value_buffer);
              int count3 = str_split(value_buffer, sizeof(value_buffer), '#',
                                     sections3, 3);
              if (count3 == 3) {
                section_str(sections3[0], value_buffer, item_buffer,
                            sizeof(item_buffer));
                printf("x1:%s\n", item_buffer);
                section_str(sections3[1], value_buffer, item_buffer,
                            sizeof(item_buffer));
                printf("x2:%s\n", item_buffer);
                section_str(sections3[2], value_buffer, item_buffer,
                            sizeof(item_buffer));
                printf("x3:%s\n", item_buffer);
              }
            }
          }
        }
        memset(TcpClient_Buff, 0, sizeof(TcpClient_Buff));
        TcpClient_BuffIndex = 0;
      }
    }
    // if (millis() - preHeartTick >= KEEPALIVEATIME) {  // 保持心跳
    //   preHeartTick = millis();
    //   Serial.println("--Keep alive:");
    //   sendtoTCPServer("ping\r\n");
    // }
    if (millis() - sendTick >= SEND_IVEATIME) {
      sendTick = millis();
      char msgBuffer[100];
      sprintf(msgBuffer, "cmd=2&uid=%s&topic=%s&msg=%d#%d#%d\r\n", UID, TOPIC,
              fw_value, lr_value, beep);
      sendtoTCPServer(msgBuffer);
      printf("send value\n");
    }
  }
}

void startSTA() {
  WiFi.disconnect();
  WiFi.mode(WIFI_STA);
  WiFi.begin(DEFAULT_STASSID, DEFAULT_STAPSW);
}
#endif

/**************************************************************************
                                 WIFI
***************************************************************************/
/*
  WiFiTick
  检查是否需要初始化WiFi
  检查WiFi是否连接上，若连接成功启动TCP Client
  控制指示灯
*/
#ifndef __USE_ESP_NOW__
void doWiFiTick() {
  static bool startSTAFlag = false;
  static bool taskStarted = false;
  static uint32_t lastWiFiCheckTick = 0;

  if (!startSTAFlag) {
    startSTAFlag = true;
    startSTA();
    Serial.printf("Heap size:%d\r\n", ESP.getFreeHeap());
  }

  // 未连接1s重连
  if (WiFi.status() != WL_CONNECTED) {
    if (millis() - lastWiFiCheckTick > 1000) {
      lastWiFiCheckTick = millis();
    }
  }
  // 连接成功建立
  else {
    if (taskStarted == false) {
      taskStarted = true;
      Serial.print("\r\nGet IP Address: ");
      Serial.println(WiFi.localIP());
      startTCPClient();
      configTime(gmtOffset_sec, daylightOffset_sec, ntpServer);
    }
  }
}
#endif
#ifdef __USE_ESP_NOW__
// callback when data is sent from Master to Slave
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x", mac_addr[0],
           mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  Serial.print("Last Packet Sent to: ");
  Serial.println(macStr);
  Serial.print("Last Packet Send Status: ");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success"
                                                : "Delivery Fail");
}
void sendData() {
  const uint8_t *peer_addr = slave.peer_addr;
  char msgBuffer[15];
  sprintf(msgBuffer, "%d#%d#%d", fw_value, lr_value, beep);
  esp_err_t result =
      esp_now_send(peer_addr, (uint8_t *)msgBuffer, sizeof(msgBuffer));
  Serial.print("Send Status: ");
  if (result == ESP_OK) {
    Serial.println("Success");
  } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
    // How did we get so far!!
    Serial.println("ESPNOW not Init.");
  } else if (result == ESP_ERR_ESPNOW_ARG) {
    Serial.println("Invalid Argument");
  } else if (result == ESP_ERR_ESPNOW_INTERNAL) {
    Serial.println("Internal Error");
  } else if (result == ESP_ERR_ESPNOW_NO_MEM) {
    Serial.println("ESP_ERR_ESPNOW_NO_MEM");
  } else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
    Serial.println("Peer not found.");
  } else {
    Serial.println("Not sure what happened");
  }
}

#endif
void setup() {
  Serial.begin(115200);
  printf("setup  .......   \n");
  pinMode(7, INPUT_PULLUP);
  analogSetAttenuation(ADC_11db);
#ifdef __USE_ESP_NOW__
  WiFi.mode(WIFI_STA);
  esp_wifi_set_channel(CHANNEL, WIFI_SECOND_CHAN_NONE);
  printf("MAC:%s\n", WiFi.macAddress().c_str());
  InitESPNow();
  esp_now_register_send_cb(OnDataSent);
#endif
}

/*
按键矩阵
6  R1
8  R2
11 R3 --> 2 ?

12 C1 --> 7
13 C2 --> 3
18 C3 --> 10
*/
int parse_analog_value(int value) {
  if (value > ANALOG_MAX) {
    value = ANALOG_MAX;
  }
  if (value < ANALOG_MIN) {
    value = ANALOG_MIN;
  }
  if (value > ANALOG_LOW && value < ANALOG_HIGH) {
    return 0;
  }
  if (value < ANALOG_LOW) {
    return -((ANALOG_LOW - value) * 100 / (ANALOG_LOW - ANALOG_MIN));
  }
  if (value > ANALOG_HIGH) {
    return (value - ANALOG_HIGH) * 100 / (ANALOG_MAX - ANALOG_HIGH);
  }
}
void rocker() {
  // 前进：IO2 HEIGH
  // 后退：IO2 LOW
  // 左转：IO3 LOW
  // 右转：IO3 HEIGH
  int io2_value = analogReadMilliVolts(2);
  int io3_value = analogReadMilliVolts(3);
  printf("io2:%d, io3:%d\n", io2_value, io3_value);
  fw_value = parse_analog_value(io2_value);
  // if (fw_value > 0 && fw_value < 28) {
  //   fw_value = 28;
  // } else if (fw_value > -28 && fw_value < 0) {
  //   fw_value = -28;
  // }
  lr_value = parse_analog_value(io3_value);
  if(beep && digitalRead(7)){
    delay(100);
    if(digitalRead(7)){
      beep = 0;
    }
  }else if(!beep && !digitalRead(7)){
      delay(100);
    if(!digitalRead(7)){
      beep = 1;
    }
  }
  printf("fw:%d, lr:%d, beep: %d\n", fw_value, lr_value, beep);
}
void loop() {
#ifndef __USE_ESP_NOW__
  doWiFiTick();
  doTCPClientTick();
#endif
  rocker();
#ifdef __USE_ESP_NOW__
  if (slave.channel == CHANNEL) {  // check if slave channel is defined
    // `slave` is defined
    // Add slave as peer if it has not been added already
    bool isPaired = manageSlave();
    if (isPaired) {
      // pair success or already paired
      // Send data to device
      sendData();
    } else {
      // slave pair failed
      Serial.println("Slave pair failed!");
      ScanForSlave();
    }
  } else {
    // No slave found to process
    ScanForSlave();
  }
#endif
}
